﻿#region COPYRIGHT
//--------------------------------------------------------------------------------
// <copyright file="SeparatingAxisTheorem.cs" company="starLiGHT Entertainment Studios">
//    Copyright (c) 2010, 2011
//       Roland Rosenkranz (Glatzemann@email.de)
// </copyright>
// <license>
//   This file is part of starLiGHT.Collision.
//
//   starLiGHT.Collision is free software: you can redistribute it and/or modify
//   it under the terms of the GNU Lesser General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
//   starLiGHT.Collision is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//   GNU Lesser General Public License for more details.
//
//   You should have received a copy of the GNU Lesser General Public License
//   along with starLiGHT.Collision. If not, see http://www.gnu.org/licenses/.
//    
//   ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
// </license>
// <version>
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 620                   $:  Revision of last commit *
// * $Author:: unknown            $:  Author of last commit   *
// * $Date:: 2010-12-07 15:23:58 #$:  Date of last commit     *
// ************************************************************
// </version>
//--------------------------------------------------------------------------------
#endregion

namespace starLiGHT.Collision.NarrowPhase
{
    #region Using Statements
    using System;
    using System.Collections.Generic;
    using System.Text;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;
    #endregion

    public class SeparatingAxisTheorem : INarrowPhase
    {
        public int CollisionCount { get; set; }

        public CollisionManifold<ICollidable> ProcessPair(CollisionManager collisionManager, UserPair<ICollidable> pair)
        {
            if (pair == null)
            {
                throw new ArgumentNullException("pair");
            }

            if (pair.object0 is ICollidable2D && pair.object1 is ICollidable2D)
            {
                return this.ProcessPair(collisionManager, pair, pair.object0 as ICollidable2D, pair.object1 as ICollidable2D);
            }

            throw new NotImplementedException("Only 2D SAT implemented at the moment.");
        }

        public bool CheckCompatibility(ICollidable c)
        {
            try
            {
                if (c is ICollidable2D)
                {
                    ICollidable2D c2d = c as ICollidable2D;
                    return ((ICollidable2D)c).Hull != null && c2d.Hull.IsConvex;
                }
            }
            catch
            {
            }

            return false;
        }

        private CollisionManifold<ICollidable> ProcessPair(CollisionManager collisionManager, UserPair<ICollidable> pair, ICollidable2D object0, ICollidable2D object1)
        {
            if (object0.Hull == null && object1.Hull == null)
            {
                throw new NotSupportedException("SeparatingAxisTheorem collision is not supported because one of the colliding objects in the CollisionPair2D has no Hull2D.");
            }

            bool intersect = true;
            bool willIntersect = true;

            int edgeCountA = object0.Hull.EdgeCount;
            int edgeCountB = object1.Hull.EdgeCount;

            float minIntervalDistance = float.PositiveInfinity;
            Vector2 translationAxis = Vector2.Zero;
            Vector2 edge;

            for (int edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
            {
                if (edgeIndex < edgeCountA)
                {
                    edge = object0.Hull.GetEdge(edgeIndex);
                }
                else
                {
                    edge = object1.Hull.GetEdge(edgeIndex - edgeCountA);
                }

                Vector2 axis = edge.Perpendicular();
                axis.Normalize();

                float minA = 0;
                float minB = 0;
                float maxA = 0;
                float maxB = 0;
                this.ProjectHull(axis, object0.Hull, object0.Transform.Position.Value - object0.Transform.Origin.Value, ref minA, ref maxA);
                this.ProjectHull(axis, object1.Hull, object1.Transform.Position.Value - object1.Transform.Origin.Value, ref minB, ref maxB);

                if (this.IntervalDistance(ref minA, ref maxA, ref minB, ref maxB) > 0f)
                {
                    intersect = false;
                }

                float velocityProjection = Vector2.Dot(axis, object0.Velocity);

                if (velocityProjection < 0f)
                {
                    minA += velocityProjection;
                }
                else
                {
                    maxA += velocityProjection;
                }

                float intervalDistance = this.IntervalDistance(ref minA, ref maxA, ref minB, ref maxB);
                if (intervalDistance > 0f)
                {
                    willIntersect = false;
                }

                if (!intersect && !willIntersect)
                {
                    break;
                }

                intervalDistance = Math.Abs(intervalDistance);
                if (intervalDistance < minIntervalDistance)
                {
                    minIntervalDistance = intervalDistance;
                    translationAxis = axis;

                    Vector2 d = (object0.Hull.Center + object0.Transform.Position.Value) -
                                (object1.Hull.Center + object1.Transform.Position.Value);
                    if (Vector2.Dot(d, translationAxis) < 0f)
                    {
                        translationAxis = -translationAxis;
                    }
                }
            }

            pair.object0.Colliding = intersect || willIntersect;
            pair.object1.Colliding = intersect || willIntersect;

            if (intersect || willIntersect)
            {
                CollisionManifold<ICollidable> manifold = new CollisionManifold<ICollidable>();
                manifold.NarrowPhaseCollision = intersect;
                manifold.Object0 = object0;
                manifold.Object1 = object1;

                manifold.Separation = translationAxis * minIntervalDistance;
                manifold.Overlap = minIntervalDistance;

                collisionManager.OnCollision(pair, manifold);

                return manifold;
            }
            else
            {
                collisionManager.OnCollisionResolved(pair);

                return null;
            }
        }

        private void ProjectHull(Vector2 axis, Hull2D polygon, Vector2 positionOffset, ref float min, ref float max)
        {
            float dotProduct = Vector2.Dot(axis, polygon[0] + positionOffset);
            min = dotProduct;
            max = dotProduct;

            for (int i = 0; i < polygon.VertexCount; i++)
            {
                dotProduct = Vector2.Dot(polygon[i] + positionOffset, axis);
                min = MathHelper.Min(min, dotProduct);
                max = MathHelper.Max(max, dotProduct);
            }
        }

        private float IntervalDistance(ref float minA, ref float maxA, ref float minB, ref float maxB)
        {
            if (minA < minB)
            {
                return minB - maxA;
            }
            else
            {
                return minA - maxB;
            }
        }
    }
}
